classdef GapCrossingLogic < PlatformLogic & VideoLogic & AnalogInLogic

  properties
    AvailableStates = {'Starting','Stopping','Ready','Active','Busy','Waiting','TrialActive','Saving'};
  end
    
  methods
    % CONSTRUCTOR
    function O = GapCrossingLogic(varargin)
      global CG;
      % Call Superclass constructors
      O@PlatformLogic; 
      O@VideoLogic; 
      O@AnalogInLogic(...
        'Device',CG.Parameters.Setup.Audio.Device{2},...
        'SRAI',CG.Parameters.Setup.Audio.SRAI,...
        'ChAI',CG.Parameters.Setup.Audio.ChAI,...
        'ChDO',[],'ChAO',[],...
        'TerminalConfig','SingleEnded',...
        'AINames',{'MicLeft','MicRight'},...
        'ControlInterval',0.25,...
        'Name','Audio',...
        'Saving','All');
      
      O = setName(O,'GapCrossing');
      P = parsePairs(varargin);
      O.ParametersFull = {...
        'ControlInterval', 0.1,'Numeric',inf;...
        'TrialDurationMax',     60,            'Numeric',inf;...
        'DistMean',                25,            'Numeric',inf;...
        'DistSD',                    4,            'Numeric',inf;...
        'DistMax',                  60,          'Numeric',inf};
    
      % HW RELATED
      O.HW.ArduinoIDs = [O.HW.IDs.ArduinoPlat,O.HW.IDs.ArduinoCam];
      O.HW.NArduinos = length(O.HW.ArduinoIDs);
      O.HW.PlatformIDs = struct('Guest',1,'Host',2); % Contains the current status of the platforms    
      
      % ANIMAL TRACKING (Animal Position on the grid: [-3,-2.5,-2,-1.5,-1,0,1,1.5,2,2.5,3];)
      O.States.AnimalPositionY = [-9:0];
      O.States.AnimalPositionX = [linspace(-3,-1,O.HW.NAnimalSensors-1),0,linspace(1,3,O.HW.NAnimalSensors-1)];
      O.States.AnimalPosition = zeros(length(O.States.AnimalPositionY),length(O.States.AnimalPositionX)); % Tracks the animal position over time
      O.States.EstAnimalPosition = -1;
      O.States.AnimalPositionByPType = 'Host';
      O.States.InputStates = zeros(1,length(O.HW.Inputs));
      % DISPLAY
      O.Display.LastFigureUpdate = now*CG.Misc.DateNum2SecFactor; % Time when the last figure was updated
       
      O.assignParameters;
    end
    
    % START THE PARADIGM
    function start(O)
      global CG;
      C_setDigitalNI(O.HW.IDs.DAQ,'Trial',0);
      C_setDigitalNI(O.HW.IDs.DAQ,'Camera',0);
      start@Logic(O) % can be used to use the subclass and the class function
      % INITIALIZE THE SYSTEM
      O.assignParameters;
      O.States.AnimalPosition(:) = 0;
      O.startAcquisition(O.HW.IDs.DAQ);
      % SHOW FIGURES
       for iM=1:length(O.Modules)
        eval(['C_show',O.Modules(iM).Type,'(',n2s(O.Modules(iM).IDType),',0);']);
       end
       
       O.HW.LastStopTrial = -1;
       
      %INITIALIZE SETUP
      pause(1.5); % wait for Arduino to boot
      O.moveGates([1,2],'closed');
      O.prepareDisplay; O.showDisplay(1);
      O.updateStates;
      O.initializePlatforms;
      O.initializeCamera;
      O.createTrialTimer;
      
      % PREPARE FOR FIRST TRIAL
      O.prepareTrial;
      pauseUntil(O.TimeToContinue);
      
      Choice = questdlg('Please put the animal on the left platform',...
        'Animal Dialog','Continue','Break','Continue');
      switch Choice;  case 'Break'; return; end
      O.startTrial;
    end
    
    function stop(O)
      C_setDigitalNI(O.HW.IDs.DAQ,'Trial',0);
      O.stopTrial;
      O.moveGates([1,2],'closed');
      O.deleteTimers;
    
      stop@Logic(O);
    end

    % PREPARE THE NEXT TRIAL
    function prepareTrial(O)
      global CG;
      
      % DELETE THE TIMERS THAT COULD HAVE STARTED THE TRIAL
    
      % PREPARE FILES AND INCREASE TRIAL
      prepareTrial@Logic(O);
       
      % PRECOMPUTE PROPERTIES
      O.updatePlatformStatus;      % ASSIGN HOST AND GUEST PLATFORM
      O.computeTrialParameters; % NEW DISTANCE & CAMERA POSITIONS
      
      % RECONFIGURE SPACE
      Distance =  abs(O.Trials(O.Trial).GuestPosition - O.HW.Platforms(O.HW.PlatformIDs.Guest).Position);
      cPausePlatform = Distance*O.HW.Platforms(O.HW.PlatformIDs.Guest).TimePerMM;
      
      Distance =  abs(O.Trials(O.Trial).CameraPosition - O.HW.Camera(1).Position);
      cPauseCamera = Distance*O.HW.Camera(1).TimePerMM;
      cPause = max([cPausePlatform,cPauseCamera]);
      
      O.TimeToContinue = now*86400 + cPause;
      
      Distance =  O.Trials(O.Trial).GuestPosition - O.HW.Platforms(O.HW.PlatformIDs.Guest).Position;
      O.movePlatform(O.HW.PlatformIDs.Guest,Distance);
      Distance = O.Trials(O.Trial).CameraPosition - O.HW.Camera.Position;
      O.moveCamera(O.HW.Camera.CameraID,Distance);
    
      O.prepareVideo(O.HW.IDs.Camera);
      O.prepareAnalogIn(O.HW.IDs.AnalogIn);
    
      disp('prepareTrial finished.')
    end
     
    function startTrial(O)
      O.Trials(O.Trial).Attempt = 0;
      O.updateDisplay;
      
      O.startAnalogIn(O.HW.IDs.AnalogIn);
      
      % START VIDEO IF ANIMAL ALREADY AT THE FRONT GATE

      States = O.updateStates;   O.updateAnimalPosition;
      Animal  = O.getInputByName(['AniPosP',num2str(O.HW.PlatformIDs.Host),'S3']);
      if ~States(Animal)   O.startVideo(O.HW.IDs.Camera);    end
      
      % SEND TRIAL START SIGNAL 
       startTrial@Logic(O);
       O.stopTrialTimer; O.startTrialTimer;
       C_setDigitalNI(O.HW.IDs.DAQ,'Trial',1);
       fprintf(['\n\n> Starting Trial ',num2str(O.Trial),'\n']);
       O.moveGates([O.HW.PlatformIDs.Guest,O.HW.PlatformIDs.Host],'open');
    end
    
    % STOP THE CURRENT TRIAL
    function stopTrial(O)
      global CG
      O.TrialActive = 0; O.changeState('Busy');
     
      % PAUSE VIDEO ACQUISITION
      SaveVideo = O.VideoActive; 
      if O.VideoActive;      O.pauseVideo(O.HW.IDs.Camera);     end    
      
      % STOP AUDIO ACQUISITION
      SaveAnalogIn = O.AnalogInActive; 
      if O.AnalogInActive O.stopAnalogIn(O.HW.IDs.AnalogIn); end
   
      % CLOSE GATE
      O.moveGates([O.HW.PlatformIDs.Guest,O.HW.PlatformIDs.Host],'closed');          
      
      % SELECT FRAME FOR VIDEO TO SAVE
      O.selectFramesForSaving(SaveVideo)
        
      % WRAP UP TRIAL
      O.updatePerformance;
      O.stopTrialTimer;
      stopTrial@Logic(O);
      disp('stopTrial finished.')
    end
    
    % COMPUTE NEW POSITIONS OF PLATFORMS & CAMERA
    function computeTrialParameters(O)
      % COMPUTE POSITIONS FOR PLATFORMS
      DistanceFound = 0;
      HostID = O.HW.PlatformIDs.Host;
      GuestID = O.HW.PlatformIDs.Guest;
      
      HostPos = O.HW.Platforms(HostID).Position;
      MinDist = HostPos + sum([O.HW.Platforms.PositionOffset]);
      RangeMaxGuest =  O.HW.Platforms(O.HW.PlatformIDs.Guest).RangeMax;
      while ~DistanceFound
        NewDistance = randn*O.Parameters.DistSD + O.Parameters.DistMean;
        if NewDistance >= MinDist && NewDistance <= MinDist+RangeMaxGuest;
          DistanceFound = 1;
        end
      end
      % ROUND DISTANCE TO 0.1MM
      NewDistance = round(10*NewDistance)/10+0.1;
      fprintf(['Distance for Trial ',num2str(O.Trial),' :  ',num2str(NewDistance),'mm\n']);
      
      GuestPos = NewDistance - MinDist;
      if GuestPos < 0 fprintf('Guest platform position below lower limit!\n'); keyboard; end
      
      HostPos = O.HW.Platforms(HostID).Position;
      if HostPos < 0 fprintf('Host platform position below lower limit!\n'); keyboard; end      
      
      % COMPUTE NEW POSITION FOR THE CAMERA
      switch GuestID
        case 1; % Target on the left
          NewCamPos = O.HW.Camera.DistanceToCenter + ...
            (GuestPos + O.HW.Platforms(GuestID).PositionOffset - O.HW.Camera.DistanceToPlatform ) ; 
        case 2; %Target on the right
          NewCamPos = O.HW.Camera.DistanceToCenter - ...
            (GuestPos + O.HW.Platforms(GuestID).PositionOffset - O.HW.Camera.DistanceToPlatform );
      end
      if NewCamPos < 0 fprintf('Camera position below lower limit!\n'); keyboard; end
      if NewCamPos > O.HW.Camera(1).RangeMax;
        fprintf('Camera position above upper range limit!\n'); keyboard; end
      O.Trials(O.Trial).Distance = NewDistance;
      O.Trials(O.Trial).GuestPosition = GuestPos;
      O.Trials(O.Trial).HostPosition = HostPos;
      O.Trials(O.Trial).CameraPosition = NewCamPos;
      O.Trials(O.Trial).Attempt = 0;
    end
    
    % UPDATE THE PERFORMANCE OF THE ANIMAL OVER TRIALS
    function updatePerformance(O)
      iT = O.Trial;
      if iT>0
        Outcome = double(strcmp(O.States.AnimalPositionByPType,'Guest'));
        O.Performance(iT).Outcome = Outcome;
      end
    end
        
    % UPDATE ANIMAL POSITION
    function updateAnimalPosition(O)
      InputStates = O.States.InputStates(O.HW.AnimalSensorsPhys);
      InputStates = ~InputStates;
      if sum(InputStates) % IF AT LEAST ONE SENSOR IS ACTIVE
        O.States.AnimalPosition(end+1,[1:2:end]) = InputStates/sum(InputStates);
      else % NO SENSOR ACTIVE
        if max(O.States.AnimalPosition(end,:))==1 % 
          T = zeros(1,size(O.States.AnimalPosition,2));
          for i=1:size(O.States.AnimalPosition,2)
            C1 = 0; if i>1; C1 = O.States.AnimalPosition(end,i-1); end
            C2 = 0; if i<size(O.States.AnimalPosition,2);  C2 = O.States.AnimalPosition(end,i+1); end
            T(i) = C1 + C2;
          end
          O.States.AnimalPosition(end+1,:) = T/sum(T);
        end
      end
      O.States.EstAnimalPosition = sum(O.States.AnimalPositionX.*O.States.AnimalPosition(end,:));
      if O.States.EstAnimalPosition<0; PID = 1; end
      if O.States.EstAnimalPosition>0; PID = 2; end;
      if O.States.EstAnimalPosition==0;  PID = 0; end;
      if O.HW.PlatformIDs.Host == PID; PType = 'Host'; else PType = 'Guest'; end
      if O.States.EstAnimalPosition==0; PType = 'Middle'; end;
      O.States.AnimalPositionByPType = PType;
    end
    
    % UPDATE PLATFORM STATUS
    function updatePlatformStatus(O)
      if O.States.EstAnimalPosition<=0 || O.Trial==1 % ANIMAL ON THE LEFT OR FIRST TRIAL
        O.HW.PlatformIDs.Host = 1; O.HW.PlatformIDs.Guest = 2;
      else % ANIMAL ON THE RIGHT
        O.HW.PlatformIDs.Host = 2; O.HW.PlatformIDs.Guest = 1;
      end
    end
    
    % EVENT PROCESSING (CORE FUNCTION)
    function processEvent(O,SourceName,Event)
      global CG;
      if ~O.ParadigmActive return; end
      switch SourceName
        case 'Keyboard';
          processEvent@Logic(O,SourceName,Event);
          
        case 'NIDAQ';
%           RelEventTime =  (now - CG.Sessions.NI(1).TriggerTime)*86400 - Event.Time(1);
%           if  RelEventTime > 1; 
%             disp(['Ignoring Event ',num2str(RelEventTime)]);
%             return; 
%           else
%             disp(['NIDAQ Event received at relative time ',num2str(RelEventTime)]);
%           end
          InputStates = O.updateStates(Event.Time(1));
          O.updateAnimalPosition;
          O.updateDisplay;
          HostID = O.HW.PlatformIDs.Host;
          GuestID = O.HW.PlatformIDs.Guest;
          switch Event.Name
            case 'AI_to_High'; % Thresholded change of analog channels
              switch Event.Data % Channel Number that has changed
                case {1,5}; % Outer End
                case {2,6}; % Middle
                case {3,7}; % Inner End
                  %                   if O.TrialActive
                  %                     switch O.States.AnimalPositionByPType,
                  %                       case 'Host'; % Animal still on the Host platform,but left the front
                  %
                  %                       case {'Guest','Middle'};
                  %                         cSensor = O.getInputByName(['AniPosP',num2str(GuestID),'S3']);
                  %                         if (Event.Time(1) - O.HW.LastStopTrial > 1) && ~InputStates(cSensor) % ANIMAL HAS ARRIVED AT GUEST
                  %                           disp('Animal Crossed (signalled by Departure at Host)!');
                  %                           O.TrialActive = 0;O.HW.LastStopTrial = Event.Time(1);
                  %                           C_setDigitalNI(O.HW.IDs.DAQ,[1,2],[0,0]);
                  %
                  %                           %C_setDigitalNI(O.HW.IDs.DAQ,'Camera',0);
                  %                           %C_setDigitalNI(O.HW.IDs.DAQ,'Trial',0);
                  %                           evalin('base',['T= timer(''TimerFcn'',[''global CG; CG.Paradigm.stopTrial; CG.Paradigm.prepareTrial; pauseUntil(CG.Paradigm.TimeToContinue); CG.Paradigm.startTrial;''],''StartDelay'',0.5,''Name'',''AnimalCrossedHost''); start(T);'])
                  %                         end
                  %                     end
                  %                   end
              end % END SWITCH EVENT NUMBER
              
            case 'AI_to_Low';  % Thresholded change of analog channels
              switch Event.Data % Channel Number that has changed
                case {1,5}; % Outer End.
              
                case {2,6}; % Middle
                  if O.TrialActive
                    switch O.States.AnimalPositionByPType,
                      case {'Guest','Middle'}; % ANIMAL HAS BROKEN BEAM ON OTHER SIDE
                        if  (Event.Time(1) - O.HW.LastStopTrial > 5)
                          disp('Animal Crossed (signalled by Arrival at Guest)!');
                          O.TrialActive = 0; O.HW.LastStopTrial = Event.Time(1);
                          if ~O.VideoActive  C_setDigitalNI(O.HW.IDs.DAQ,[2],1); pause(0.01); end; % STRANGE CASE, WHERE VIDEO NOT TRIGGERED
                          C_setDigitalNI(O.HW.IDs.DAQ,[1,2],[0,0]);
                          disp('Setting Camera & Trial Trig to 0');
                          %C_setDigitalNI(O.HW.IDs.DAQ,'Trial',0);
                          evalin('base',['T= timer(''TimerFcn'',[''global CG; CG.Paradigm.stopTrial; CG.Paradigm.prepareTrial; pauseUntil(CG.Paradigm.TimeToContinue);  CG.Paradigm.startTrial;''],''StartDelay'',0.3,''Name'',''AnimalCrossedGuest''); start(T);'])
                        end
                    end
                  end
                  
                case {3,7}; % Inner End
                  if O.TrialActive
                    switch O.States.AnimalPositionByPType,
                      case 'Host'; % ANIMAL ATTEMPTS TO CROSS
                        if ~O.VideoActive && O.VideoReady
                          O.startVideo(O.HW.IDs.Camera);
                          O.Trials(O.Trial).Attempt = O.Trials(O.Trial).Attempt + 1;
                          disp('Video Starting!');
                        end
                    end
                  end
              end
          end % END SWITCH EVENT TYPE
      end
    end
     
    % PREPARE DISPLAY BEFORE FIRST PLOT
    function prepareDisplay(O)
      if ~isfield(O.Display,'Handles') || ~ishandle(O.Display.Handles.FIG)
        prepareDisplay@Logic(O); 
      else
        figure(O.Display.Handles.FIG); 
      end
      clf;
      DC = axesDivide(1,[1.2,1,1],[0.1,0.2,0.8,0.7],[],1);
      colormap(HF_colormap({[1,1,1],[1,0,0]},[0,1]));
      % ANIMAL POSITION
      FontSize = 8;
      O.Display.Handles.AnimalAxis = axes('Pos',DC{1},'FontSize',FontSize);
      O.Display.Handles.AnimalPosPlot = imagesc(O.States.AnimalPositionX,O.States.AnimalPositionY,O.States.AnimalPosition(end-9:end,:));
      set(O.Display.Handles.AnimalAxis,'CLim',[0,1],'XLim',[O.States.AnimalPositionX([1,end])],'YLim',O.States.AnimalPositionY([1,end]));
      O.Display.Handles.AnimalTitle = get(O.Display.Handles.AnimalAxis,'Title');
      set(O.Display.Handles.AnimalTitle,'String',['Animal Position']);
      
      % PERFORMANCE STATISTICS
      % successful crossings and time until crossing over the history
      O.Display.Handles.AnimalPerfAxis = axes('Pos',DC{2},'FontSize',FontSize);
      O.Display.Handles.PerfH  = plot(0,0,'.-r');
      %O.Display.Handles.TimeH = plot(Time,zeros(size(Time)),'-.r');
      set(O.Display.Handles.AnimalPerfAxis,'YLim',[-0.1,1.1]);
      O.Display.Handles.PerfTitle = get(O.Display.Handles.AnimalPerfAxis,'Title');
      set(O.Display.Handles.PerfTitle,'String',['Performance']);
      
      % DISTANCE STATISTICS
      O.Display.Handles.AnimalHistAxis = axes('Pos',DC{3},'FontSize',FontSize);
      Bins = [0:5:100];
      O.Display.Handles.HistH = bar(Bins,zeros(size(Bins)));
      O.Display.DistBins = Bins;
      set(O.Display.Handles.AnimalHistAxis,'YLim',[0,5],'XLim',Bins([1,end]));
      O.Display.Handles.HistTitle = get(O.Display.Handles.AnimalHistAxis,'Title');
      set(O.Display.Handles.HistTitle,'String',['Distance Distribution']);
     
      % CURRENT TRIAL INFORMATION
      % text fields which contain the information 
      Fields = {'Distance','Host','Guest'};
      for iF=1:length(Fields)
        O.Display.Handles.([Fields{iF},'Text']) = text(0.5+(iF-2)*0.4,-0.8,[Fields{iF}],'Units','norm','FontSize',8,'Horiz','center');
      end
      O.Display.LastFigureUpdate = now;
      set(O.Display.Handles.FIG,'Visible','off');
    end
    
    % UPDATE FIGURE WHENEVER NECESSARY
    function updateDisplay(O)
      global CG
      cTime = now*CG.Misc.DateNum2SecFactor;
      % LIMIT UPDATE RATE TO CONTROLINTERVAL
      if get(CG.GUI.Main.Paradigm.ShowButton,'Value') ...
          && cTime - O.Display.LastFigureUpdate > O.Parameters.ControlInterval;
        
        % ANIMAL POSITION
        set(O.Display.Handles.AnimalPosPlot,'CData',O.States.AnimalPosition(end-9:end,:));
        set(O.Display.Handles.AnimalTitle,'String',['Animal Position : ',O.States.AnimalPositionByPType]);
        
        % PERFORMANCE STATISTICS
        if length(O.Performance)>0
          Trials = [1:length(O.Performance)]; Crossings = [O.Performance.Outcome];
          set(O.Display.Handles.PerfH,'XData',Trials,'YData',Crossings);
        end
        
        % HISTOGRAM OF GAP DISTANCES
        if O.Trial && ~isempty(O.Trials)
          H = hist([O.Trials.Distance],O.Display.DistBins);
          set(O.Display.Handles.HistH,'YData',H);
        end
        
        % UPDATE POSITION INFORMATION
        if O.Trial  && ~isempty(O.Trials) && length(O.Trials)==O.Trial
          set(O.Display.Handles.DistanceText,'String',['Distance : ',n2s(O.Trials(O.Trial).Distance),'mm']);
        end
        set(O.Display.Handles.HostText,'String',  ['Host : ',n2s(O.HW.PlatformIDs.Host),' ( ',O.HW.Platforms(O.HW.PlatformIDs.Host).Location,' )']);
        set(O.Display.Handles.GuestText,'String',['Guest : ',n2s(O.HW.PlatformIDs.Guest),' ( ',O.HW.Platforms(O.HW.PlatformIDs.Guest).Location,' )']);
        
        O.Display.LastFigureUpdate = cTime;
      end
     % catch exception
      %  fprintf(['ERROR : (while updateDisplay) : ',exception.stack(1).name,' ',num2str(exception.stack(1).line),': ',exception.message,'\n']);
    %end
    end
  end
end